#include "toroidal.h"
#include <math.h>
#include "cudafunc.h"
#include <gsl/gsl_sf_hermite.h>
#include <gsl/gsl_poly.h>
#include <gsl/gsl_integration.h>
#include <gsl/gsl_spline.h>

extern field Apsi;

double qfunc(double psip, void* params)
{
    double psi_poly[7] = {2.148, -4.946, 37.47, -130, 269.3, -275.7, 109.5};
    return gsl_poly_eval(psi_poly,7,psip);
}

// nh is hermite function order, mp is poloidal mode number, rho0 mode location, shear is inverse mode width
void perturb_hermitefunc(fiq* gen, int nh, int mp, double rho0, double shear, bool iq)
{
    gsl_integration_workspace* w = gsl_integration_workspace_alloc(1000);
    gsl_function F;
    F.function = &qfunc;
    F.params = nullptr;
    double* psit = new double[101];
    double* psin = new double[101];
    double err;
    for(int i = 100; i >= 0; i--)
    {
        double psicur = (double)i*0.01;
        psin[i] = psicur;
        gsl_integration_qags(&F, 0, psicur, 0, 1e-7, 1000,w, psit+i, &err);
    }
    for(int i = 0; i < 101; i++)
        *(psit+i) = sqrt(*(psit+i))/sqrt(*(psit+100));
    
    gsl_interp_accel *acc = gsl_interp_accel_alloc();
    gsl_spline *spl = gsl_spline_alloc(gsl_interp_cspline, 101);
    gsl_spline_init(spl, psin, psit, 101);
    printf("safety factor:%f\n", qfunc(rho0, nullptr));

    double* datai = gen->get_fi();
    double* dataq = gen->get_fq();
    double mag = 1e-1;
    double psi0 = -0.3;
    double r0 = 4.23;
    double z0 = 0.0;
    for (int i = 0; i < Nx*Ny; i++)
    {
        double rloc = (i/Ny)*dr + Rmin;
        double zloc = (i%Ny)*dz + Zmin;
        cylv vp = {rloc, 0, zloc};
        rloc = rloc - r0;
        if (!checkboundary(vp))
            continue;
        double psiloc = Apsi.interp(vp);
        double radius = gsl_spline_eval(spl, 1.0+(psiloc-1e-10)/(-psiaxis+1e-10), acc);
        double radiumag = mag*gsl_sf_hermite_func(nh, shear*(radius-rho0));
        double theta = atan(zloc/rloc) +  pi*(rloc<0.0) + 2*pi*(zloc < 0.0 && rloc > 0.0);
        if(iq == true)
        {
            datai[i] = radiumag*cos(mp*theta);
            dataq[i] = -radiumag*sin(mp*theta);
        }
        else
        {
            datai[i] = radiumag*sin(mp*theta);
            dataq[i] = radiumag*cos(mp*theta);
        }
    }
    gsl_spline_free (spl);
    gsl_interp_accel_free (acc);
    return;
    
}

// https://zhuanlan.zhihu.com/p/20374706 Hammersley sequence

double integerRadicalInverse(int Base, int i)
{
	int numPoints, inverse;
	numPoints = 1;
	for(inverse = 0; i > 0; i /= Base)
	{
		inverse = inverse * Base + (i % Base);
		numPoints = numPoints * Base;
	}
	return inverse / (double) numPoints;
}

void ptcs::init_hammersley(field B)
{
    double R0 = 4.34;
    double radius = 0.3;
    for(int i = 0; i < npt/2; i++)
    {
        double xi0 = (double)i/(double)npt;
        double xi1 = integerRadicalInverse(2,i);
        double xi2 = integerRadicalInverse(3,i);
        double xi3 = integerRadicalInverse(5,i);
        double xi4 = integerRadicalInverse(7,i);
        double xi5 = integerRadicalInverse(11,i);
        double r = R0 + radius*sqrt(xi5)*cos(xi2*(2.0*pi));
        double z = radius*sqrt(xi5)*sin(xi2*(2.0*pi));
        double v = pow(exp(3*xi3) - 1, 1.0/3);

        double Bloc = B.interp({r,0.0,z});
        double mux = 0.5*v*v*(2*xi4-xi4*xi4)/Bloc;
        this->ppts[i] = particle({r,xi1*2*pi,z}, mux, v*(1-xi4));
        this->ppts[i+npt/2] = particle({r,xi1*2*pi,z}, mux, -v*(1-xi4));
    }
}

void ptcs::init_realistic(field B)
{
    double R0 = 4.34;
    double radius = 1.5;
    int i = 0;
    int accind = 0;
    while(accind < npt/2)
    {
        double xi0 = (double)accind/(double)npt;
        double xi1 = integerRadicalInverse(2,i);
        double xi2 = integerRadicalInverse(3,i);
        double xi3 = integerRadicalInverse(5,i);
        double xi4 = integerRadicalInverse(7,i);
        double xi5 = integerRadicalInverse(11,i);
        double xi6 = integerRadicalInverse(13,i++);
        double r = R0 + radius*sqrt(xi5)*cos(xi2*(2.0*pi));
        double z = radius*sqrt(xi5)*sin(xi2*(2.0*pi));
        double v = pow(exp(3*xi3) - 1, 1.0/3);

        double Bloc = B.interp({r,0.0,z});
        double mux = 0.5*v*v*(2*xi4-xi4*xi4)/Bloc;
        double prob = this->distribfunc({{r,xi1*2*pi,z}, mux, v*(1-xi4)});
        if(prob > xi6 && checkboundary_core({r,xi1*2*pi,z}))
        {
            this->ppts[accind] = particle({r,xi1*2*pi,z}, mux, v*(1-xi4));
            this->ppts[accind+npt/2] = particle({r,xi1*2*pi,z}, mux, -v*(1-xi4));
            inside[accind] = true;
            inside[accind+npt/2] = true;
            accind++;
            
        }
    }
}

void ptcs::init_timeindependent(world wld)
{
    int i = 0;
    int accind = 0;
    while(accind < npt)
    {
        i++;
        double xi1 = integerRadicalInverse(2,i);
        double xi2 = integerRadicalInverse(3,i);
        double xi3 = integerRadicalInverse(5,i);
        double r = xi1*(Rmax-Rmin)*0.9 + Rmin+0.1;
        cylv crd0 = {r, 0.0, 0.0};
        double Bloc = wld.Bsca->interp(crd0);
        double v = exp(xi2)/2.718;
        double mux = 0.5*v*v*(2*xi3-xi3*xi3)/Bloc;
        double vpara = v*(1-xi3);

        particle pa = particle(crd0, mux, vpara);
        double dt = 0.01;
        bool osde = true;
        int nt = 0;
        while (osde == true)
        {
            double vpar = pa.getvpara();
            cylv ccp = pa.getcrd();
            osde = checkboundary_core(ccp);
            vec cp_xyz = cyl2xyz(ccp);
            pfvec em1 = wld.emfield(ccp);
            phsp dX_phsp1 =wld.push_rk2(em1, &pa, dt, vpar);
            cylv crdn = xyz2cyl(cp_xyz + dX_phsp1.X);
            pa.setcrd(crdn, vpar+dX_phsp1.vp);
            if(crdn.z * ccp.z < 0 || nt > 50000)
                break;
            nt++;
        }
        
        if (osde == true && nt < 50000)
        {
            int interval = 1024;
            int nptgen = 0;
            for(int pt = 0; pt < nt; pt ++)
            {
                double vpar = pa.getvpara();
                cylv ccp = pa.getcrd();
                vec cp_xyz = cyl2xyz(ccp);
                pfvec em1 = wld.emfield(ccp);
                phsp dX_phsp1 =wld.push_rk2(em1, &pa, dt, vpar);
                cylv crdn = xyz2cyl(cp_xyz + dX_phsp1.X);
                pa.setcrd(crdn, vpar+dX_phsp1.vp);
                if(pt > nptgen*interval)
                {
                    ppts[accind] = particle(ccp,mux,vpar);
                    inside[accind++] = true;
                    nptgen++; 
                }
            }
        }
    }
    printf("total particle generated:%d",i);
    
}